/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package edu.wpi.first.wpilibj.templates;


import edu.wpi.first.wpilibj.CANJaguar;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.SimpleRobot;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.Watchdog;
import edu.wpi.first.wpilibj.can.CANTimeoutException;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the SimpleRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class RobotTemplate extends SimpleRobot {
    /**
     * This function is called once each time the robot enters autonomous mode.
     */
    private CANJaguar FrontRight, FrontLeft, BackRight, BackLeft, Shooter1, Shooter2;
    private Victor BD, Conveyor, conveyorPanel, turntableMotor;
    private Joystick Drive, shooter;
    public void autonomous() {
        
    }

    /**
     * This function is called once each time the robot enters operator control.
     */
    public void operatorControl() {
        Drive = new Joystick(1);
        shooter = new Joystick(2);
        BD = new Victor(2,1);
        Conveyor = new Victor(2,2);
        conveyorPanel = new Victor(2,3);
        turntableMotor = new Victor(2,4);
        try {   
            FrontRight = new CANJaguar(1); //Initialize front right motor
            FrontLeft = new CANJaguar(2); //Initialzie front left motor
            BackRight = new CANJaguar(3); //Initialize back right motor
            BackLeft = new CANJaguar(4); //Initialize back left motor
            Shooter1 = new CANJaguar(5);
            Shooter2 = new CANJaguar(6);
            FrontRight.setVoltageRampRate(0.0);
            FrontLeft.setVoltageRampRate(0.0);
            BackRight.setVoltageRampRate(0.0);
            BackLeft.setVoltageRampRate(0.0);
        } catch (CANTimeoutException GGYOUFAIL) {
            
        }
        while(true){
            //Drive
            double x, y, z;
            Watchdog.getInstance().feed();
            x = Drive.getX(); //Combined X Axis Input
            y = Drive.getY();
            z = Drive.getZ();
            try{
                FrontRight.setX(-(y - x + z)); //set front right motor to normalized calculated speed
                FrontLeft.setX(y + x - z); //set front left motor to normalized calculated speed
                BackRight.setX(-(y + x - z)); //set back right motor to normalized calculated speed
                BackLeft.setX(y - x - z); //set back left motor to normalized calculated speed
            } catch (CANTimeoutException ex) {
            }
            //Shooter
            double speed;
            speed = shooter.getThrottle();
            try{
                Shooter1.setX(speed);
                Shooter2.setX(speed);
            } catch (CANTimeoutException ex) {
            }
            //Bridge Depressor
            boolean down, up;
            down = Drive.getRawButton(6) || Drive.getRawButton(5);
            up = Drive.getRawButton(3) || Drive.getRawButton(4);
            if((up && down) || (!up && !down)){
                BD.set(0.0);
            } else if(!up && down){
                BD.set(0.2);
            } else if(up && !down){
                BD.set(-0.2);
            }
            //Conveyor Panel
            boolean out, in;
            out = shooter.getRawButton(5);
            in =  shooter.getRawButton(4);
            if((in && out) || (!in && !down)){
                conveyorPanel.set(0.0);
            } else if(!in && out){
                conveyorPanel.set(-0.08);
            } else if(in && !out){
                conveyorPanel.set(0.08);
            }
            //Conveyor
            boolean Shooter, drive;
            Shooter = shooter.getRawButton(3);
            drive = Drive.getRawButton(1);
            if(Shooter || drive){
                Conveyor.set(0.5);
            }
            //Turntable
            turntableMotor.set(shooter.getX());
        }
    }
    private double limit(double input){
        if(input > 1.0){
            return 1.0;
        } else if(input < -1.0){
            return -1.0;
        } else {
            return input;
        }
    }
}
